﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Drifting.MotionPlanning.Systems;
using Drifting.Graphics.EffectObjects;
using Drifting.Graphics.PhysicalObjects.Vehicles;
using Drifting.Foundation;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;

using System.Threading;
using Drifting.MotionPlanning.Primitives;
using Drifting.Graphics.Visualizers;
using Drifting.MotionPlanning.SingleThreading.Planners;
using Drifting.MotionPlanning.SingleThreading.Algorithms;
using System.Diagnostics;

namespace Drifting.Scenes
{
    public class BiPedal_plan : Basic
    {
        OneLink oneLink;

        RenderTarget2D target_perspective;
        Rectangle rec_perspective;

        RenderTarget2D target_phaseA;
        Rectangle rec_phaseA;
        Vector2 canvas_horizontal;
        Vector2 canvas_vertical;
        Vector2 viewport_horizontal;
        Vector2 viewport_vertical;

        SpriteBatch spriteBatch;
        SpriteFont spriteFont;

        Matrix phaseA_view;
        Matrix phaseA_project;

        Thread planning_thread;

        Line2D l_axis;
        Line2D l_p;
        Line2D l_red;
        Line2D l_top;
        Line2D l_bottom;
        Line2D l_nominal;

        List<double[]> point_list;
        List<double[]> point_list2;


        public BiPedal_plan(ref Matrix view, ref Matrix projection, GraphicsDevice device, Camera camera, Game1 game, SpriteFont spriteFont)
            : base(ref view, ref projection, device, camera, game)
        {
            this.camera = camera;
            spriteBatch = new SpriteBatch(device);
            this.spriteFont = spriteFont;

            camera.Camera_InitView(2f, 1.2f, 5f, 2, 0, 0, (float)(Math.PI / 3.2f), ref view, ref projection);
            //grid.XYZ = false;
            grid.setGrid(20, 0.2f, 8, 0.03);


            oneLink = new OneLink_simple();


            point_list = new List<double[]>();
            point_list2 = new List<double[]>();
            l_axis = new Line2D(0.02f, Color.Black, device);
            l_p = new Line2D(0.01f, Color.Blue, device);
            l_red = new Line2D(0.04f, Color.Red, device);
            l_nominal = new Line2D(0.03f, Color.Green, device);
            l_top = new Line2D(0.03f, Color.HotPink, device);
            l_bottom = new Line2D(0.03f, Color.Blue, device);

            target_phaseA = new RenderTarget2D(
                device,
                800,
                800,
                false,
                device.PresentationParameters.BackBufferFormat,
                DepthFormat.Depth24,
                4,
                RenderTargetUsage.DiscardContents);

            rec_phaseA = new Rectangle(0, 0, 800, 800);

            phaseA_project = Matrix.CreateOrthographic(8f, 20f, -1, 5);    //1.8f, 10, -1, 5
            phaseA_view = Matrix.CreateLookAt(
                new Vector3(0f, 0, 1),
                new Vector3(0f, 0, 0),
                new Vector3(0, 1, 0));

            canvas_horizontal = new Vector2(-10, 10);
            canvas_vertical = new Vector2(-5, 5);
            viewport_horizontal = new Vector2(5.5f, 7.3f);
            viewport_vertical = new Vector2(-4, 4);


            target_perspective = new RenderTarget2D(
                device,
                800,
                533,
                false,
                device.PresentationParameters.BackBufferFormat,
                DepthFormat.Depth24,
                8,
                RenderTargetUsage.DiscardContents);
            rec_perspective = new Rectangle(0, 0, 800, 533);




            ThreadStart threadStart_Action = new ThreadStart(Planning);
            planning_thread = new Thread(threadStart_Action);
            planning_thread.IsBackground = true;
            planning_thread.Start();
        }

        bool phaseA_init = false;
        private void Draw_phaseA(Matrix camera, Matrix projection)
        {
            device.SetRenderTarget(target_phaseA);

            if (phaseA_init == false)
            {
                device.DepthStencilState = new DepthStencilState() { DepthBufferEnable = true };
                device.Clear(Color.GhostWhite);
                phaseA_init = true;
            }
            device.Clear(Color.GhostWhite);

            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(-10, 0, 0), new Vector3(10f, 0, 0));
            l_axis.Draw(phaseA_view, phaseA_project, new Vector3(0, 10, 0), new Vector3(0f, -10, 0));

            float x_p = (float)Math.PI / 2;
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(x_p, 10, 0), new Vector3(x_p, -10, 0));
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(-x_p, 10, 0), new Vector3(-x_p, -10, 0));
            x_p = (float)oneLink.Max_Alias[0];
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(x_p, 10, 0), new Vector3(x_p, -10, 0));
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(-x_p, 10, 0), new Vector3(-x_p, -10, 0));

            float y_p = (float)oneLink.Max_Alias[1];
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(-10, y_p, 0), new Vector3(10, y_p, 0));
            l_p.Draw(phaseA_view, phaseA_project, new Vector3(-10, -y_p, 0), new Vector3(10, -y_p, 0));




            if (point_list2.Count > 0)
            {
                lock (point_list2)
                {
                    for (int i = 0; i < point_list2.Count ; ++i)
                    {
                        double[] p0 = point_list2[i];
                        
                        l_red.Draw(phaseA_view, phaseA_project,
                            new Vector3((float)(p0[0] - 0.02), (float)p0[1], 0),
                            new Vector3((float)(p0[0] + 0.02), (float)p0[1], 0));
                    }
                }

            }

            if (point_list.Count > 1)
            {
                for (int i = 0; i < point_list.Count - 1; ++i)
                {
                    double[] p0 = point_list[i];
                    double[] p1 = point_list[i + 1];
                    double dist = MathAssistant.Euclidean(p0, p1);
                    if (dist < 5)
                        l_nominal.Draw(phaseA_view, phaseA_project, new Vector3((float)p0[0], (float)p0[1], 0), new Vector3((float)p1[0], (float)p1[1], 0));
                }
            }



            device.SetRenderTarget(null);
        }

        override public void Draw(Matrix camera, Matrix projection)
        {
            Draw_phaseA(camera, projection);
            //Draw_perspective(camera, projection);


            spriteBatch.Begin(SpriteSortMode.Immediate, BlendState.AlphaBlend,
                SamplerState.LinearClamp, DepthStencilState.Default,
                RasterizerState.CullNone);

            spriteBatch.Draw(target_phaseA, rec_phaseA, Color.White);
            spriteBatch.Draw(target_perspective, rec_perspective, Color.White);


            spriteBatch.End();

        }


        public void Regulator(List<double[]> pi_nominal, double[] x, out double[] u)
        {
            double m1 = 1;
            double L1 = 1;
            double g = 9.8;
            double Dt = 0.002;
            double[] x_next = new double[2];
            //  dx = gx + b*u;

            double gx0 = x[1];
            double gx1 = -g * Math.Cos(x[0]) * 1.5 / L1;

            double b0 = 0;
            double b1 = 3 / (m1 * L1 * L1);

            double torque_max = oneLink.Max_Control[0];
            double torque_min = oneLink.Min_Control[0];

            double torque = torque_max;
            x_next[0] = x[0] + gx0 * Dt + b0 * torque * Dt;
            x_next[1] = x[1] + gx1 * Dt + b1 * torque * Dt;

            double d_x = get_min_euclidean_to_pi_from_x(pi_nominal, x);
            double d_next_1 = get_min_euclidean_to_pi_from_x(pi_nominal, x_next);

            torque = torque_min;
            x_next[0] = x[0] + gx0 * Dt + b0 * torque * Dt;
            x_next[1] = x[1] + gx1 * Dt + b1 * torque * Dt;

            double d_next_2 = get_min_euclidean_to_pi_from_x(pi_nominal, x_next);


            u = new double[1];

            double d_min = double.MaxValue;
            if (d_next_1 < d_next_2)
            {
                u[0] = torque_max;
                d_min = d_next_1;
            }
            else
            {
                u[0] = torque_min;
                d_min = d_next_2;
            }

            if (d_min > d_x)
            {
                d_min = d_x;
                u[0] = 0;
            }

        }

        double get_min_euclidean_to_pi_from_x(List<double[]> pi_nominal, double[] x)
        {
            double d_min = double.MaxValue;
            for (int i = 0; i < pi_nominal.Count; ++i)
            {
                double d_i = MathAssistant.Euclidean(pi_nominal[i], x);
                if (d_min > d_i)
                    d_min = d_i;
            }

            return d_min;
        }


        public void Planning()
        {
            double duration_inc = 0.001;
            Random rand = new Random((int)DateTime.Now.Ticks);
            BaseSystem.MemoryPad pad = new BaseSystem.MemoryPad();
            pad.Prepare(2);
            point_list = new List<double[]>();
            point_list2 = new List<double[]>();

            double[] q_start = new double[2];
            q_start[0] = Math.PI / 6;
            q_start[1] = 2.5;

            double duration = 0.75;
            double[] q_next = new double[2];
            double[] q_tmp = new double[2];
            double[] u = new double[2];
            MathAssistant.vector_set(q_start, ref q_tmp);

            for (double t = 0; t < duration; t += duration_inc)
            {
                double step_cost = 0;
                oneLink.Propagate(pad, q_tmp, u, duration_inc, ref q_next, ref step_cost);
                MathAssistant.vector_set(q_next, ref q_tmp);

                double[] p = new double[2];
                MathAssistant.vector_set(q_next, ref p);

                point_list.Add(p);
            }

            int n_try = 0;
            while (true)
            {
                n_try++;
                double duration2 = 2.5;
                q_next = new double[2];
                q_tmp = new double[2];
                u = new double[2];
                double[] q_start2 = new double[2];


                oneLink.SampleState(rand, ref q_start2);


                MathAssistant.vector_set(q_start2, ref q_tmp);
                for (double t = 0; t < duration2; t += duration_inc)
                {

                    Regulator(point_list, q_tmp, out u);


                    double step_cost = 0;
                    oneLink.Propagate(pad, q_tmp, u, duration_inc, ref q_next, ref step_cost);
                    MathAssistant.vector_set(q_next, ref q_tmp);

                    double[] p = new double[3];
                    MathAssistant.vector_set(q_next, ref p);


                    double dist = get_min_euclidean_to_pi_from_x(point_list, q_next);
                    if (dist < 0.01)
                    {
                        lock (point_list2)
                        {
                            point_list2.Add(q_start2);

                            double converge_rate = Math.Truncate(point_list2.Count / (double)n_try * 1000) / 10;
                            Debug.WriteLine("converge rate: " + converge_rate + "%\t " + point_list2.Count + "\t in "+ n_try);
                        }

                        break;
                    }
                }
                
            }

        }
    }
}
